#include "imu.h"
#include "L3G4200D.h"
#include "LSM303A.h"
#include "LSM303M.h"
#include "math.h"
#include "unistd.h"
//#include <fcntl.h>
#include <stdio.h>
#include "rpi.h"

#include <stdlib.h>

#define DEBUG false


#include "imuconstants.h"




/*
   Gyro sensitivity scales: 
     1 -- 250 dps (degree per second)
     2 -- 500 dps
     3 -- 2000 dps
*/
#define IMU_GYRO_SCALE 1

#if IMU_GYRO_SCALE == 1
  #define IMU_GYRO_GAIN 0.00875
  #define IMU_GYRO_CONF 0x00
#elif IMU_GYRO_SCALE == 2
  #define IMU_GYRO_GAIN 0.01750
  #define IMU_GYRO_CONF 0x10
#else
  #define IMU_GYRO_GAIN 0.070
  #define IMU_GYRO_CONF 0x20
#endif



#define IMU_INIT_READINGS 15








IMU::IMU()
{
  device_acc = new LSM303A(RPi::i2cFile());
  printf("IMU: creator complete\n");
  device_mag = new LSM303M(RPi::i2cFile());
  printf("IMU: creator complete\n");
  device_gyr = new L3G4200D(RPi::i2cFile());
  printf("IMU: creator complete\n");
  prev_t = 0;
  printf("IMU: creator complete\n");

  baserot = Quaternion(1, 0, 0, 0);

  imup.setMeasurementErrors(
    Vector3D( 
      IMU_ACC_ES_X*IMU_ACC_ES_X + IMU_ACC_ED_X*IMU_ACC_ED_X,
      IMU_ACC_ES_Y*IMU_ACC_ES_Y + IMU_ACC_ED_Y*IMU_ACC_ED_Y,
      IMU_ACC_ES_Z*IMU_ACC_ES_Z + IMU_ACC_ED_Z*IMU_ACC_ED_Z
    ),
    Vector3D(
      IMU_MAG_ES_X*IMU_MAG_ES_X + IMU_MAG_ED_X*IMU_MAG_ED_X,
      IMU_MAG_ES_Y*IMU_MAG_ES_Y + IMU_MAG_ED_Y*IMU_MAG_ED_Y,
      IMU_MAG_ES_Z*IMU_MAG_ES_Z + IMU_MAG_ED_Z*IMU_MAG_ED_Z
    ),
    Vector3D(
      IMU_GYR_ES_X*IMU_GYR_ES_X + IMU_GYR_ED_X*IMU_GYR_ED_X,
      IMU_GYR_ES_Y*IMU_GYR_ES_X + IMU_GYR_ED_X*IMU_GYR_ED_X,
      IMU_GYR_ES_Z*IMU_GYR_ES_X + IMU_GYR_ED_X*IMU_GYR_ED_X
    )
  );

  facta = 0.72;
  factm = 0.42;
}

IMU::~IMU()
{
  delete device_gyr;
  delete device_mag;
  delete device_acc;
}

void IMU::setup()
{
}

void IMU::initialize()
{
  // mock readout
  usleep(IMU_MIN_SENSOR_REST*1000);
  read(); 
  usleep(IMU_MIN_SENSOR_REST*1000);


  Vector3D aa[IMU_INIT_READINGS];
  Vector3D mm[IMU_INIT_READINGS];
  Vector3D gg[IMU_INIT_READINGS];

  // store initialization readings
  for (int i = 0; i < IMU_INIT_READINGS; i++)
  {
    read();
    aa[i] = a;
    mm[i] = m;
    gg[i] = g;
    usleep(IMU_MIN_SENSOR_REST*1000);
  }

  //compute averages
  for (int i = 0; i < IMU_INIT_READINGS; i++)
  {
    avga += aa[i];
    avgm += mm[i];
    avgg += gg[i];
  }
  avga /= IMU_INIT_READINGS;
  avgm /= IMU_INIT_READINGS;
  avgg /= IMU_INIT_READINGS;
  //compute error
  double errax = 0, erray = 0, erraz = 0;
  double errgx = 0, errgy = 0, errgz = 0;
  for (int i = 0; i < IMU_INIT_READINGS; i++)
  {
    errax += (aa[i].x() - avga.x())*(aa[i].x() - avga.x());
    erray += (aa[i].y() - avga.y())*(aa[i].y() - avga.y());
    erraz += (aa[i].z() - avga.z())*(aa[i].z() - avga.z());
    errgx += (gg[i].x() - avgg.x())*(gg[i].x() - avgg.x());
    errgy += (gg[i].y() - avgg.y())*(gg[i].y() - avgg.y());
    errgz += (gg[i].z() - avgg.z())*(gg[i].z() - avgg.z());
  }
  errax /= IMU_INIT_READINGS;
  erray /= IMU_INIT_READINGS;
  erraz /= IMU_INIT_READINGS;
  errgx /= IMU_INIT_READINGS;
  errgy /= IMU_INIT_READINGS;
  errgz /= IMU_INIT_READINGS;
  errax = sqrt(errax);
  erray = sqrt(erray);
  erraz = sqrt(erraz);
  errgx = sqrt(errgx);
  errgy = sqrt(errgy);
  errgz = sqrt(errgz);
  Vector3D avgaerr(errax, erray, erraz);
  Vector3D avggerr(errgx, errgy, errgz);

/*
  avga.x() = 0.0587393;
  avga.y() = -0.063837;
  avga.z() = 1.03746;

  avgm.x() = 0.577004;
  avgm.y() = 1.0985;
  avgm.z() = 1.29255;
  old
*/

/*
  avga.x() = -0.07916;
  avga.y() = -0.00670;
  avga.z() =  0.99607;

  avgm.x() = -0.26490;
  avgm.y() =  0.13515;
  avgm.z() =  0.95218;
  */
   
  // normalize and let's hope length a was ~1 to begin with
  avga /= avga.length();
  avgm /= avgm.length();

  printf("IMU: measured inital a  ");
  avga.print();
  printf("\n");
  printf("IMU: measured inital m  ");
  avgm.print();
  printf("\n");

  // avga represents the initial accelerometer reading
  // rotate avgm correspondingly. this will become the reference magnetic field. 


  imup.vectorToQuaternion(avga, avgaerr, baserot, baseroterr);
  
  printf("IMU: quaternion base rotation (from acceleration)  ");
  baserot.print();
  printf("\n");

  a = avga;
  m = avgm;
  g = avgg;
  
  rotateToZeroFrame(a);
  rotateToZeroFrame(m);
  printf("IMU: rotated initial a  ");
  a.print();
  printf("\n");
  printf("IMU: rotated initial m  ");
  m.print();
  printf("\n");


  imup.setInitialValues(a, m, g);



  double magang = atan(m.z()/sqrt(m.x()*m.x()+m.y()*m.y()))/3.14159*180;
  printf("IMU: measured magnetic field inclination (relative to inital position): %.2f deg\n", magang);
}

void IMU::read()
{
  // update data array index

  int ACC_READINGS = 1;

  Vector3D tempa[100];
  // subsample the acceleration
  for (int i = 0; i < ACC_READINGS; i++)
  {
    device_acc->read();
    tempa[i].x() = device_acc->a.x();
    tempa[i].y() = device_acc->a.y();
    tempa[i].z() = device_acc->a.z();
  }

  // read other sensors
  device_gyr->read();
  device_mag->read();
  
  // store reading time
  unsigned long curr_t = RPi::usec();
  t = curr_t - prev_t;
  prev_t = curr_t;

  a.x() = 0;
  a.y() = 0;
  a.z() = 0;
  double w = 0;
  for (int i = 0; i < ACC_READINGS; i++)
  {
    tempa[i].x() = (tempa[i].x() - IMU_ACC_C_X) / IMU_ACC_S_X;
    tempa[i].y() = (tempa[i].y() - IMU_ACC_C_X) / IMU_ACC_S_X;
    tempa[i].z() = (tempa[i].z() - IMU_ACC_C_X) / IMU_ACC_S_X;
    double n = 1;
    //if (tempa[i].length() > 1)
    //  n = tempa[i].length();
    a.x() += tempa[i].x() / n;
    a.y() += tempa[i].y() / n;
    a.z() += tempa[i].z() / n;
    w += 1/n;
    //for (int j = 0; j < 3; j++)
    //  printf("%.7f ", tempa[i](j));
    //printf("\n");
  }
  a.x() /= w;
  a.y() /= w;
  a.z() /= w;

  Vector3D mm;
  mm.x() = (device_mag->m.x() - IMU_MAG_C_X) / IMU_MAG_S_X;
  mm.y() = (device_mag->m.y() - IMU_MAG_C_Y) / IMU_MAG_S_Y;
  mm.z() = (device_mag->m.z() - IMU_MAG_C_Z) / IMU_MAG_S_Z;

  m.x() = IMU_MAG_A_11 * mm.x() + IMU_MAG_A_12 * mm.y() + IMU_MAG_A_13 * mm.z();
  m.y() = IMU_MAG_A_21 * mm.x() + IMU_MAG_A_22 * mm.y() + IMU_MAG_A_23 * mm.z();
  m.z() = IMU_MAG_A_31 * mm.x() + IMU_MAG_A_32 * mm.y() + IMU_MAG_A_33 * mm.z();


  g.x() = (float)device_gyr->g.x() * IMU_GYRO_GAIN / 180 * 3.14159265f;
  g.y() = (float)device_gyr->g.y() * IMU_GYRO_GAIN / 180 * 3.14159265f;
  g.z() = (float)device_gyr->g.z() * IMU_GYRO_GAIN / 180 * 3.14159265f;

  rotateToZeroFrame(a);
  rotateToZeroFrame(m);
  rotateToZeroFrame(g);

  
  avga = facta * avga + (1-facta) * a;
  double fm = 0;
  for (int i = 0; i < 3; i++)
  {
    fm = fabs(m(i) - avgm(i));
    fm *= fm * 20;
    avgm(i) = factm * avgm(i) + (1-factm) * m(i);
  }
}

void IMU::rotateToZeroFrame(Vector3D &arg)
{
  Quaternion r = baserot.conjugate() * Quaternion(arg) * baserot;
  arg = Vector3D(r);
}


Vector3D IMU::getM()
{
  return m;
}
Vector3D IMU::getA()
{
  return a;
}
Vector3D IMU::getG()
{
  return g;
}
double IMU::getT()
{
  return t;
}



// execute filters. 
// assumes a[id], m[id] g[id] has been set by read()
void IMU::process()
{
  /*
  Quaternion qra, qrm, qrg;
  qra = baserot.conjugate() * Quaternion(a) * baserot;
  qrm = baserot.conjugate() * Quaternion(m) * baserot;
  qrg = baserot.conjugate() * Quaternion(g) * baserot;

  imup.process(Vector3D(qra), Vector3D(qrm), Vector3D(qrg), t);
  */
  imup.process(a, m, g, t);
}


/*
void IMU::estimateAngleSimple(const Vector3D &accinput, const Vector3D &accerr, Quaternion &angle, Quaternion &angleerr)
{
  angleerr = Quaternion();

  double pitch, roll;

  Vector3D acc = accinput;
  acc /= acc.length();

  pitch = asin(acc.x());
  roll = atan2(-acc.y(),acc.z());

  double sx, sy, sz; // acc errors.
  sx = accerr.x();
  sy = accerr.y();
  sz = accerr.z();
  // all errors are NOT squared
  double pitcherr1, pitcherr2, pitcherr;
  double rollerr1, rollerr2, rollerr;

  pitcherr1 = sx / sqrt(1 - acc.x()*acc.x());
  pitcherr2 = sqrt(acc.y()*acc.y()*sy*sy + acc.z()*acc.z()*sz*sz)/acc.x()/sqrt(1-acc.x()*acc.x());
  pitcherr = pitcherr1;
  if (pitcherr2 < pitcherr1)
    pitcherr = pitcherr2;

  rollerr1 = sqrt(sy*sy + sz*sz*acc.y()*acc.y()/acc.z()/acc.z())/(1+acc.y()*acc.y()/acc.z()/acc.z())/acc.z();
  rollerr2 = sqrt(sy*sy + pitcherr*pitcherr*acc.x()*acc.x()*acc.y()*acc.y()/(1-acc.x()*acc.x()))/acc.z();
  rollerr = rollerr1;
  if (rollerr2 < rollerr1)
    rollerr = rollerr2;

  double cp, sp, cr, sr;
  cp = cos(pitch/2);
  sp = sin(pitch/2);
  cr = cos(roll/2);
  sr = sin(roll/2);

  angle = Quaternion(
    cp*cr,
    cp*sr,
    sp*cr,
    sp*sr
  );

  // all errors become squared from now on.

  pitcherr *= pitcherr;
  rollerr *= rollerr;
   
  cp *= cp;
  sp *= sp;
  cr *= cr;
  sr *= sr;

  // angle err IS squared
  angleerr = Quaternion(
    pitcherr * cr * sp + rollerr  * cp * sr,
    rollerr  * cp * cr + pitcherr * sp * sr,
    pitcherr * cp * cr + rollerr  * sp * sr,
    rollerr  * cr * sp + pitcherr * cp * sr
  );  
  angleerr *= 0.25;

  // angle err NOT squared
  for (int i = 0; i < 4; i++)
    angleerr(i) = sqrt(angleerr(i));

}

*/


